#!/usr/bin/env python 
import rospy
import math
import os
import sys
import subprocess
import re
from geometry_msgs.msg import PoseStamped
from geometry_msgs.msg import TwistStamped

noisy_file            = open('pose_noisy.txt', 'w')
noisy_file_vel        = open('vel_noisy.txt', 'w')
ground_truth_file     = open('pose_ground_truth.txt', 'w')
ground_truth_file_vel = open('vel_ground_truth.txt', 'w')

result_file = open('result.txt', 'w')
start_time = 0

goal_x = 250.0
goal_y = 0.0

last_speed_squared = 0

crashed = False

starting_line = 5
finish_line = 155

def OnCrash(pose_msg):
  global crashed
  crashed = True

def AtGoal(pose_x, pose_y, radius):
  dx = pose_x - goal_x
  dy = pose_y - goal_y 
  distance_to_goal_squared = dx * dx + dy * dy
  return ( distance_to_goal_squared < radius * radius) and (last_speed_squared < 4.0)

def CrossedFinishLine(pose_x):
  return pose_x > finish_line

def Terminate(final_time, reason):
  global noisy_file, result_file, ground_truth_file
  print reason
  total_time = final_time - start_time
  result_file.write(str(total_time) + ',' + reason + "\n")
  noisy_file.close()
  ground_truth_file.close()
  result_file.close()

  rospy.signal_shutdown(reason)
  sys.exit()

def WritePoseToFile(pose_msg, file):
  file.write(str(pose_msg.header.stamp.to_sec()) + ',')
  file.write(str(pose_msg.pose.position.x) + ',')
  file.write(str(pose_msg.pose.position.y) + ',')
  file.write(str(pose_msg.pose.position.z) + '\n')

def WriteTwistToFile(twist_msg, file):
  file.write(str(twist_msg.header.stamp.to_sec()) + ',')
  file.write(str(twist_msg.twist.linear.x) + ',')
  file.write(str(twist_msg.twist.linear.y) + ',')
  file.write(str(twist_msg.twist.linear.z) + '\n')

def OnGoal(pose_msg):
  global goal_x, goal_y
  goal_x = pose_msg.pose.position.x
  goal_y = pose_msg.pose.position.y

def OnNoisyPose(pose_msg):
  global noisy_file
  WritePoseToFile(pose_msg, noisy_file);

def Crashed():
  return os.path.isfile('/home/peteflo/crashed.txt');

def OnNoisyTwist(twist_msg):
  global noisy_file_vel
  WriteTwistToFile(twist_msg, noisy_file_vel);

def OnRealPose(pose_msg):
  global ground_truth_file, start_time
  if start_time == 0 and pose_msg.pose.position.x > starting_line:
    start_time = pose_msg.header.stamp.to_sec()
    print "CROSSED STARTING LINE"

  WritePoseToFile(pose_msg, ground_truth_file);

  if Crashed():
    Terminate(pose_msg.header.stamp.to_sec(), "FAIL")
    print "CRASH DETECTED"
  elif CrossedFinishLine(pose_msg.pose.position.x):
    print "CROSSED FINISH LINE"
    Terminate(pose_msg.header.stamp.to_sec(), "SUCCESS")

def OnRealTwist(twist_msg):
  global ground_truth_file_vel, last_speed_squared

  last_speed_squared = twist_msg.twist.linear.x*twist_msg.twist.linear.x + twist_msg.twist.linear.y*twist_msg.twist.linear.y 
  WriteTwistToFile(twist_msg, ground_truth_file_vel)

rospy.init_node('experiment_logger')
rospy.Subscriber('/FLA_ACL02/pose', PoseStamped, OnNoisyPose)
rospy.Subscriber('/FLA_ACL02/vel', TwistStamped, OnNoisyTwist)
rospy.Subscriber('/true_pose', PoseStamped, OnRealPose)
rospy.Subscriber('/true_vel', TwistStamped, OnRealTwist)
rospy.Subscriber('/move_base_simple/goal', PoseStamped, OnGoal)
rospy.Subscriber('/unity_crash', PoseStamped, OnCrash)
rospy.spin()
